//*****************************************************************
// Project2.cpp
//
//  Created on: Jan 21, 2012
//      Author: Christopher Wood
//              Robert Livosi
//
//  $Id$
//*****************************************************************

// Module includes
#include "Project2.h"

#include <fstream>
#include <iostream>
#include <string>
#include <sstream>

#include <hw/inout.h>
#include <sys/neutrino.h>

#include <sys/siginfo.h>
#include <sys/netmgr.h>
#include <sys/neutrino.h>
#include <hw/inout.h>
#include <stdint.h>
#include <sys/mman.h>

#include <pthread.h>

//DAC Configuration Registers
#define DAC_BASE                 (0x280)
#define DAC_ANALOG_INPUT_STATUS	 (0x283)
#define DAC_LSB					 (0x286)
#define DAC_MSB_Channel			 (0x287)
#define WAIT_DAC				 (0x10)  // bit-4 of base+3 to wait for DAC to update


// Constants used by the ADDriver
#define DIO_CTRL_ADDRESS (0x28B)
#define PORTA_ADDRESS (0x288)
#define PORTB_ADDRESS (0x289)
#define PORTC_ADDRESS (0x28A)
#define DATA_ADDRESS (0x378) // Data register address
#define CTRL_ADDRESS (0x37a) // Control register address
#define STUS_ADDRESS (0x379) // Status register address
#define INIT_BIT (0x04) // Init bit for the paralle port (pin 2)
#define INTR_BIT (0x10) // Interrupt bit for the paralle port (pin 4)
#define PARALLEL_IRQ (0x07) // Parallel port IRQ
#define LOW (0x00) // Low signal value for parallel port
#define HIGH (0xFF) // Low signal value for parallel port
#define TIMEOUT_ROLLOVER (4) // The timeout rollover
#define BYTE_PATTERN (0x19)
#define ONE_SECOND (1000000000)
#define MIN_VOLTAGE_VAL (-5)
#define MAX_VOLTAGE_VAL (5)
#define CONVERSION_FACTOR (25.4) // -127 to 127 for servo positions

#define BASE_ADDRESS					(0x280)
#define LSB_BASE_ADDRESS				(BASE_ADDRESS)
#define MSB_BASE_ADDRESS				(BASE_ADDRESS + 0x01)
#define BASE_OUTPUT_VAL					(0x80)
#define AD_CHANNEL_ADDRESS				(BASE_ADDRESS + 0x02)
#define AD_CHANNEL_VAL					(0x44)
#define ANALOG_INPUT_STATUS_ADDRESS		(BASE_ADDRESS + 0x03)
#define ANALOG_INPUT_STATUS_VAL			(0x00)
#define ANALOG_INPUT_WAIT_BIT			(0b00100000)
#define AD_RANGE						(32768)
#define INPUT_RANGE						(10)

#define PORT_LENGTH	(0x01)

// Private data handles that are shared by various routines inside the converter
uintptr_t lsbHandle;
uintptr_t msbHandle;
uintptr_t adChannelHandle;
uintptr_t analogInputStatusHandle;

ofstream myfile;

// Perform an A/D conversion to get a digital value from the
// input signal.
//--------------------------------------------------------------
void PerformADConversion(void)
{
	while ((in8(analogInputStatusHandle) & ANALOG_INPUT_WAIT_BIT) != 0)
	{
		// busy waiting
	}

	// start the A/D conversion
	out8(lsbHandle, 0x10);
	out8( lsbHandle, BASE_OUTPUT_VAL );

	while ((in8(analogInputStatusHandle) & BASE_OUTPUT_VAL) != 0)
	{
		// busy waiting
	}
	//cout << (int)(in8(analogInputStatusHandle)) << endl;
}

// Retrieve the 16-bit A/D value.
//--------------------------------------------------------------
uintptr_t fifoHandle;
uintptr_t basep1Handle;
int16_t ReadADData(void)
{
	int counter = 0;
	int8_t lsbData = in8(lsbHandle);
	int8_t msbData = in8(msbHandle);
	int16_t dataToReturn = (msbData << 8) + lsbData;

	/*for (counter = 0; counter < 48; counter++)
	{
		lsbData = in8(lsbHandle);
		msbData = in8(msbHandle);

		dataToReturn = (msbData << 8) + lsbData;
	}*/
	//out8(lsbHandle, 0x10);

	//cout << dataToReturn << endl;

	return dataToReturn;
}

// Convert a 16-bit A/D value to a digital voltage level.
//--------------------------------------------------------------
float ConvertADData(int16_t adBoardData)
{
	double convertedInputVoltage = (double)(adBoardData * INPUT_RANGE) / AD_RANGE;
	return convertedInputVoltage;
}

#define OUTPUT_SIZE (3) // only need 3 in the past
int uIndex = 0;
int eIndex = 0;
int outputIndex = 0;
float output[OUTPUT_SIZE];
float u[OUTPUT_SIZE];
float e[OUTPUT_SIZE];

#define DIVIDER (10)
float kd = 0;
float kp = 1.0;
float ki = 0;
float rz = 2.5;

int inputIndex = 0;
float adInputBuffer[OUTPUT_SIZE];

int CLIP_CUSHION = 10.0;
int flag = 0;
int resetTime = 30;
int currentResetTime = 0;

void timerExpired(sigval arg)
{
	uint8_t LSB, MSB, channel;
	int16_t adReturnValue;
	int16_t outValue;
	float adInput;
	float error;
	float diff;

	PerformADConversion();
	adReturnValue = ReadADData();
	adInput = ConvertADData(adReturnValue); // this serves as A/D input

	adInputBuffer[inputIndex] = adInput;
	diff = adInput - adInputBuffer[(inputIndex + OUTPUT_SIZE - 1) % OUTPUT_SIZE];
	if (diff < 0)
		diff *= -1;

	// Check for overflow
	if (flag == 1 && (currentResetTime < resetTime))
	{
		outValue = 2048;
		currentResetTime++;

		kp = 1.0;
		ki = 0;
		kd = 0;
		rz = 0;
	}
	else if (diff > CLIP_CUSHION)
	{
		flag = 1;
		currentResetTime = 0;
		outValue = 2048;
	}
	else
	{
		// Rest clip flag
		flag = 0;

		// Calculate error
		error = rz - adInput;

		// Perform the conversion
		u[uIndex] = ((((kp + ki + kd) * error) -
				(kp + (2 * kd)) * e[(eIndex + OUTPUT_SIZE - 1) % OUTPUT_SIZE] +
				(kd * e[(eIndex + OUTPUT_SIZE - 2) % OUTPUT_SIZE]))) +
				u[(uIndex + OUTPUT_SIZE - 1) % OUTPUT_SIZE];

		// Keep only the low 8 bits
		if ((u[uIndex]) > 9)
		{
			outValue = (int)(((9.0 / INPUT_RANGE) * 2048) + 2048);
			u[uIndex] = 9.0;
		}
		else if ((u[uIndex]) < -9)
		{
			outValue = (int)(((-9.0 / INPUT_RANGE) * 2048) + 2048);
			u[uIndex] = -9.0;
		}
		else
		{
			outValue = (int)((((u[uIndex]) / INPUT_RANGE) * 2048) + 2048);
		}

		// Update indices
		e[eIndex] = error;
		uIndex = (uIndex + 1) % OUTPUT_SIZE;
		eIndex = (eIndex + 1) % OUTPUT_SIZE;
	}

	// Clear
	inputIndex = (inputIndex + 1) % OUTPUT_SIZE;

	// Strip off the low 8 bits, keep 4 high bits
	LSB = outValue & 255;
	MSB = (outValue >> 8) & 15;

	// Select Channel 0
	channel = 0;
	out8(DAC_LSB, LSB);
	out8(DAC_MSB_Channel, MSB + (channel << 6));

	// Wait for the D/A to update
	while (in8(DAC_ANALOG_INPUT_STATUS) & WAIT_DAC);
}

void* userInputThread(void* arg)
{
	while (true)
	{
		cout << "rz = ";
		cin >> rz;

		// get rid of all old data
		//memset(u, 0, sizeof(float) * OUTPUT_SIZE);
		//memset(e, 0, sizeof(float) * OUTPUT_SIZE);

		cout << "kp = ";
		cin >> kp;
		cout << "ki = ";
		cin >> ki;
		cout << "kd = ";
		cin >> kd;
	}

	return NULL;
}

int main(int argc, char *argv[])
{
	uintptr_t dioControlHandle;
	uintptr_t ainteHandle;
	uint8_t LSB, MSB, channel;
	int16_t adReturnValue;
	int16_t outValue;
	float adInput;
	float error;
	int counter = 0;

	memset(adInputBuffer, 0, sizeof(float) * OUTPUT_SIZE);

	// Get permission to access the hardware
	int privity_err = ThreadCtl(_NTO_TCTL_IO, NULL);
	if (privity_err == -1) {
		cerr << "Can't get root permissions" << endl;
		exit(-1);
	}
	cout << "Starting..." << std::endl;

	// Get a handle to the DIO control register
	dioControlHandle = mmap_device_io(PORT_LENGTH, DIO_CTRL_ADDRESS);

	// Configure the DIO channels according to the protocol
	// A-output (A/D values), B-output (done bit), C-output (comm status)
	out8(dioControlHandle, 0x00);

	// Get a handle to the LSB of the A/D register
	lsbHandle = mmap_device_io( PORT_LENGTH, LSB_BASE_ADDRESS );

	// Get a handle to the MSB of the A/D register
	msbHandle = mmap_device_io( PORT_LENGTH, MSB_BASE_ADDRESS );

	// Get a handle to the A/D channel register
	adChannelHandle = mmap_device_io( PORT_LENGTH, AD_CHANNEL_ADDRESS );

	// Set the board to the specified channel(s)
	out8(adChannelHandle, 0x00);

	// Get a handle to the analog input status register
	analogInputStatusHandle = mmap_device_io( PORT_LENGTH, ANALOG_INPUT_STATUS_ADDRESS );
	ainteHandle = mmap_device_io(PORT_LENGTH, ANALOG_INPUT_STATUS_ADDRESS + 1);
	fifoHandle = mmap_device_io(PORT_LENGTH, ANALOG_INPUT_STATUS_ADDRESS + 2);
	basep1Handle = mmap_device_io(PORT_LENGTH, 0x280);

	// Set A/D FIFO threshold to 1
	out8(fifoHandle, 0xFF);

	// Set AINTE to 0
	out8(ainteHandle, 0x00);

	// Select the input range for the A/D converter
	out8( analogInputStatusHandle, ANALOG_INPUT_STATUS_VAL );	// +10V, -10V range (gain of 0)

	// Structures used by the new timer
	struct itimerspec timerSpec;
	struct sigevent event;
	timer_t timerID;

	// Initialize the daemon timer thread to invoke a function when it expires
	SIGEV_THREAD_INIT(&event, &timerExpired, 0, 0);

	// Configure the timer parameters (period signals only)
	timerSpec.it_value.tv_sec = 0;
	timerSpec.it_value.tv_nsec = 100000000; // 10Hz 1 00
	timerSpec.it_interval.tv_sec = 0;
	timerSpec.it_interval.tv_nsec = 100000000; // 10Hz 1 00

	// open the file
	cout << "enter log file name: " << endl;
	string logFile = "";
	getline(cin, logFile);
	myfile.open(logFile.c_str());

	pthread_create(NULL, NULL, userInputThread, NULL);

	// Start the timer
	timer_create(CLOCK_REALTIME, &event, &timerID);
	timer_settime(timerID, 0, &timerSpec, NULL);

	// Compute the D/A code for the desired output voltage
	while (true)
	{
		/*PerformADConversion();
		adReturnValue = ReadADData();
		adInput = ConvertADData(adReturnValue);
		if (counter == 0)
			cout << adInput << endl;
		counter = (counter + 1) % 48;
		sched_yield();*/

		//outValue = (int)(((rz / INPUT_RANGE) * 2048) + 2048);
	}

	return EXIT_SUCCESS;
}
